//////////////////////////////////////////////////
// This file generates Gaussian random variables.
#include <math.h>
#include<iostream>

#include "mcRand.h"

using namespace std;

#define RANDOM_MAX 0x7FFFFFFF

static unsigned int ztu_holdrand=1;

void ztu_srand (unsigned int seed)
{
	ztu_holdrand = (int)seed;
}

int ztu_rand(void)
{
	return(((ztu_holdrand = ztu_holdrand * 214013L + 2531011L) >> 16) & 0x7fff );
}

unsigned int ztu_seed(void)
{
	return ztu_holdrand;
}

///////////////////////////////////////////////////
// Class Rand
Rand::Rand()
{
	num = -1;
	// Initialize random number generator
	ran2(num);
	// stores random number in r
	r = ran2(num);
}

// returns random number
double Rand::GetRand()
{
	return r;
}

// generates new random number, stores in r and then returns r
double Rand::NewRand()
{
	r = ran2(num);
	return r;
}

/****************************Random Number Generator******************/
double Rand::ran2(int& idum)
{
	return ((double) ztu_rand())/((double)RAND_MAX);
/*
	const int IM1=2147483563;
	const int IM2=2147483399;
	const double AM=(1.0/IM1);
	const int IMM1=(IM1-1);
	const int IA1=40014;
	const int IA2=40692;
	const int IQ1=53668;
	const int IQ2=52774;
	const int IR1=12211;
	const int IR2=3791;
	const int NTAB=32;
	const int NDIV=(1+IMM1/NTAB);
	const double EPS=1.2e-7;
	const double RNMX=(1.0-EPS);


	int j;
	int k;
	static int idum2=123456789;
	static int iy=0;
	static int iv[NTAB];
	double temp;

	if(idum <= 0)
	{
		if(-(idum) < 1)	idum = 1;
		else			idum = -(idum);

		idum2 = (idum);
		for(j=NTAB+7;j >= 0;j--)
		{
			k=(idum)/IQ1;
			idum=IA1*(idum-k*IQ1)-k*IR1;
			if(idum < 0)	idum+=IM1;
			if(j < NTAB)	iv[j] =idum;
		}

		iy = iv[0];
	}

	k = idum/IQ1;
	idum = IA1*(idum - k*IQ1) - k*IR1;
	if(idum < 0)	idum += IM1;

	k = idum2/IQ2;
	idum2 = IA2*(idum2-k*IQ2)-k*IR2;
	if(idum2 < 0)	idum2 += IM2;
	j = iy/NDIV;
	iy = iv[j]-idum2;
	iv[j] = idum;
	if (iy < 1)	iy += IMM1;
	if ((temp = AM*iy) > RNMX)	return RNMX;
	else						return temp;
*/
}


/////////////////////////////////////////////
// Class Gauss

// constructor takes the mean value and the standard deviation of the desired distribution
// and then stores the first Gaussian random number in variable g.  Sets count to zero.
Gauss::Gauss(double m, double s)
      : Rand()
{
	mean = m;
	sigma = s;
	g = box_muller(mean, sigma);
}

// get one gaussian random variable
double Gauss::GetOne(void)
{
	g = box_muller(mean, sigma);
	return g;
}

double Gauss::GetOne(const double m, const double s)
{
	mean = m;
	sigma = s;
	return GetOne();
}

// get gaussian random matrix
mcmcMatrix<double> Gauss::GetMatrix(const int m, const int n)
{
	assert(m>0 && n>0);			// valid matrix

	mcmcMatrix<double> randn(m,n);
	for (int j = 0; j < m; j++)
		for (int i = 0; i < n; i++)
			randn(j,i) = GetOne();
	return randn;
}

// box_muller function takes two random numbers with uniform distribution and returns
// a random number with Gaussian distribution.  It also stores the random number in
// the variable g.
double Gauss::box_muller(double m, double s)
{
	double x1, x2, w, y1;
	static double y2;
	static int use_last = 0;

	// use value from previous call

	if (use_last)
	{
		y1 = y2;
		use_last = 0;
	}
	else
	{
		do {
				x1 = 2.0 * NewRand() - 1.0;
				x2 = 2.0 * NewRand() - 1.0;
				w = x1 * x1 + x2 * x2;
			} while ( w >= 1.0 );

		w = sqrt( (-2.0 * log( w ) ) / w );
		y1 = x1 * w;
		y2 = x2 * w;
		use_last = 1;
	}
	return( m + y1 * s );
}

//////////////////////////////////////////////////////
// prior probability

// construction & destruction
RF::RF()
{
	count = 0;
	x0 = 0;
	d = 1;
}

RF::~RF()
{
	count = 0;
	x0 = 0;
	d = 1;
}

///////////////////////////////////////////
// operators
RF& RF::operator=(const RF &x)
{
	if (this != &x)
	{
		bins = x.bins;

		count = x.count;
		x0 = x.x0;
		d = x.d;
		cdf_bins = x.cdf_bins;
		cdf_d = x.cdf_d;
		r_g = x.r_g;
	}
	return *this;
}

ofstream & operator>>(RF &x, ofstream &strm)
{
	x.bins>>strm;
	strm.write((char*)&(x.count), sizeof(x.count));
	strm.write((char*)&(x.x0), sizeof(x.x0));
	strm.write((char*)&(x.d), sizeof(x.d));

	return strm;
	
}

ifstream & operator<<(RF &x,ifstream &strm)
{
	x.bins<<strm;
	strm.read((char*)&(x.count), sizeof(x.count));
	strm.read((char*)&(x.x0), sizeof(x.x0));
	strm.read((char*)&(x.d), sizeof(x.d));

	return strm;
	
}


// get pdf function
double RF::pdf(double x)
{
	// get index
	int i = GetIndex(x);
	if (i>=0 && i<count)
		return bins.Data(i);		// prior pdf value
	else
		return 0;
}

__forceinline void RF::SetOneBin(const double x, const double p)
{
	// get index
	int i = GetIndex(x);
	if (i>=0 && i<count)
		bins(i)=p;		// prior pdf value
}

void RF::AddOneBin(const double x, const double p)
{
	// get index
	int i = GetIndex(x);
	if (i>=0 && i<count)
		bins(i)=bins(i)+p;		// prior pdf value
}

bool RF::InRange(const double x)
{
	// get index
	int i = GetIndex(x);
	if (i>=0 && i<count)
		return true;
	else
		return false;
}

double RF::cdf(const double x)
{
	double dp=0.0;
	// get index
	int i = __min(GetIndex(x),count-1);
	for (int j=0; j<=i; j++)
		dp += bins.Data(j);
	return dp;
}

void RF::Set(const double x1, const double x2, const double d1)
{
	x0 = x1;
	d = d1;

	assert(x1<=x2);
	count = (int)floor((x2-x1)/d1)+1;
	assert(count>0);
    if (count <= 0)		// no valid bin
        return;

	bins.SetRowVector(count);
	bins.Zero();
}

void RF::Set(mcmcVector<float> &votes)
{
	Set(0,votes.size()-1,1);
	for(int i=0; i<votes.size(); i++)
		SetOneBin(i,votes(i));
	SetCdf();
}


void RF::Set(mcmcVector<int> &votes)
{
	Set(0,votes.size()-1,1);
	for(int i=0; i<votes.size(); i++)
		SetOneBin(i,votes(i));
	SetCdf();
}

void RF::InitValue(const double value)
{
	bins.InitValue(value);
}

bool RF::Read(const char *lpFile)
{
	ifstream	stream;
	double		temp;

	stream.open(lpFile);
	if (stream.is_open())
	{
		// lower bound
		stream >> x0;

		// interval
		stream >> d;

		// count
		stream >> count;

		bins.SetRowVector(count);
		for (int i=0; i<count; i++)
		{
			stream >> temp;
			bins(i) = temp;
		}
		stream.close();
		return true;
	}
	else
		return false;
}

bool RF::Save(const char *lpFile)
{
	ofstream	stream;

	stream.open(lpFile);
	if (stream.is_open())
	{
		stream << x0 << "\n" << d << "\n";
		stream << count << "\n";

		for (int i=0; i<bins.size(); i++)
			stream << bins(i) << "\n";

		stream.close();
		return true;
	}
	else
		return false;
}

// get log of pdf function
double RF::log_pdf(double x)
{
	// get index value
	int i = (int)((x-x0)/d);
	// prior pdf value
	if (i>=0 && i<count)
	{
		if (bins.Data(i)>0)
			return log(bins.Data(i));
		else
			return -INF;
	}
	else
		return -INF;
}

double	RF::pdf_T(const double x, const double T)
{
	return exp(log_pdf(x)/T);
}

// get first direvitive of log function
double RF::d1_log_pdf(double x)
{
	double y1,y2;

	y1 = log_pdf(x);
	y2 = log_pdf(x+d);
	if (y2 != -INF)
		return (y2-y1)/d;
	else
	{
		y2 = log_pdf(x-d);
		return (y1-y2)/d;
	}
}

// normalization of pdf function
bool RF::normalize(void)
{
	double	sum=0;

	sum = bins.Sum();
	if (sum>0.0)
	{
		for (int i=0; i<bins.size(); i++)
			bins(i) = bins(i)/sum;

		return true;
	}
	else
		return false;
}

void RF::SetCdf(const bool bBig)
{
	double	s=0;
	int	j=0;

	if (bBig)
		cdf_d = 0.001;
	else
		cdf_d = 0.1/((double)GetCount());
	cdf_bins.SetRowVector((int)(1/cdf_d)+1);
	cdf_bins.Zero();

	normalize();
	for (int i=0; i<bins.size(); i++)
	{
		s += bins(i);
		while (cdf_d*double(j)<s && cdf_bins.Valid(j))
		{
			cdf_bins(j) = i;
			j++;
		}
	}
}

double	RF::SamplingNonSetCdf(void)
{
	normalize();

	double	v;
	double	sum,sample;
	int	index,size;

	v=r_g.NewRand();
	sum=0.0;
	size = bins.size();
	for (index=0; index<size; index++)
	{
		sum += bins(index);
		if (sum>=v)
			break;
	}
	index = __min(index,bins.size()-1);
	sample = d*((double)index)+x0;

	// repeat the process to avoid proposing zero probability point
	if (pdf(sample)==0.0)
	{
		v=r_g.NewRand();
		sum=0.0;
		size = bins.size();
		for (index=0; index<size; index++)
		{
			sum += bins(index);
			if (sum>=v)
				break;
		}
		index = __min(index,bins.size()-1);
		sample = d*((double)index)+x0;
	}
	return sample;
}

/////////////////////////////////////////
// uniform distribution
RF_uniform::RF_uniform()
	:RF()
{
}

RF_uniform::RF_uniform(double x1,double x2,double d1)
	:RF()
{
	Set(x1,x2,d1);
}

// reset pdf function
void RF_uniform::Set(const double x1,const double x2,const double d1)
{
	u = (x1+x2)/2;
	w = x2-x1;
	d = d1;

	x0 = x1;

	// allocate space
	count = (int)(w/d+1);
	bins.SetRowVector(count);

	// initialization
	bins.InitValue(1);
	SetCdf();
}

///////////////////////////////////////////
// Gaussian distribution
RF_gaussian::RF_gaussian()
	:RF()
{
}

RF_gaussian::RF_gaussian(double u1,double sigma1,double d1)
	:RF()
{
	Set(u1,sigma1,d1);
}

void RF_gaussian::Set(const double u1,const double sigma1,const double d1)
{
	u = u1;		// mean
	sigma = sigma1;
	d = d1;

	double	x,sigma2;
	int		i;

	sigma2 = sigma*sigma;

	count = (int)(6*sigma/d+1);
	x0 = u-(double)((count-1)/2)*d;

	if (count > 0)
	{
		bins.SetRowVector(count);
		for (i=0; i<bins.size(); i++)
		{
			x = d*i+x0;
			bins(i) = 1/(sqrt(2*LPI)*sigma)*exp(-(x-u)*(x-u)/(2*sigma2));
		}
		SetCdf();
	}
}

/////////////////////////////////////////
// possion distribution
RF_possion::RF_possion()
	:RF()
{
}

RF_possion::RF_possion(double lamda1)
	:RF()
{
	Set(lamda1);
}

void RF_possion::Set(const double lamda1)
{
	lamda = lamda1;
	x0 = 0;
	d = 1;

	count = (int)(2000/lamda);
	bins.SetRowVector(count);

	double factorial=1;

	for (int i=0; i<count; i++)
	{
		bins(i) = pow(lamda,i)/(exp(lamda)*factorial);
		factorial *= i+1;
	}
	SetCdf();
}


RF_pose::RF_pose()
	:RF()
{
}

RF_pose::RF_pose(const double sigma1, const double d1)
	:RF()
{
	Set(sigma1,d1);
}

void RF_pose::Set(const double sigma1, const double d1)
{
	int		i;
	double	y,x,u;

	x0 = 0;
	d = d1;
	sigma = sigma1;

	double sigma2;
	sigma2 = sigma*sigma;

	// allocation
	count = int(2*LPI/d);
	bins.SetDimension(1,count);

	for (i=0; i<count; i++)
	{
		x = d*i-LPI;

		// four Gaussian probability at mean -pi,pi/2,0,pi/2
		u = -LPI;
		y = 0.25/(sqrt(2*LPI)*sigma)*exp(-(x-u)*(x-u)/(2*sigma2));
		u = LPI/2;
		y += 0.25/(sqrt(2*LPI)*sigma)*exp(-(x-u)*(x-u)/(2*sigma2));
		u = 0;
		y += 0.25/(sqrt(2*LPI)*sigma)*exp(-(x-u)*(x-u)/(2*sigma2));
		u = LPI/2;
		y += 0.25/(sqrt(2*LPI)*sigma)*exp(-(x-u)*(x-u)/(2*sigma2));
		u = LPI;
		y += 0.25/(sqrt(2*LPI)*sigma)*exp(-(x-u)*(x-u)/(2*sigma2));

		bins(i) = y;
	}
}



///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
// construction & destruction
RF3D::RF3D()
{
}

RF3D::~RF3D()
{
}

// operators
RF3D& RF3D::operator=(const RF3D &x)
{
	if (this != &x)
	{
		bins = x.bins;
		x1_0 = x.x1_0;
		x2_0 = x.x2_0;
		x3_0 = x.x3_0;
		d1	 = x.d1;
		d2   = x.d2;
		d3   = x.d3;
		cdf_bins = x.cdf_bins;
		cdf_d= x.cdf_d;
	}
	return (*this);
}

// operatsion
void RF3D::Set(const double x1_min, const double x1_max, const double d1_interval,
	 		   const double x2_min, const double x2_max, const double d2_interval,
			   const double x3_min, const double x3_max, const double d3_interval)
{
	int dim1,dim2,dim3;

	x1_0 = x1_min;
	d1 = d1_interval;
	dim1 = (int)floor((x1_max-x1_min)/d1)+1;
	assert(dim1>0);

	x2_0 = x2_min;
	d2 = d2_interval;
	dim2 = (int)floor((x2_max-x2_min)/d2)+1;
	assert(dim2>0);

	x3_0 = x3_min;
	d3 = d3_interval;
	dim3 = (int)floor((x3_max-x3_min)/d3)+1;
	assert(dim3>0);

	bins.SetDimension(dim1,dim2,dim3);
	bins.InitValue(0.0);
}

double RF3D::pdf(const double x1, const double x2, const double x3)
{
	int dim1,dim2,dim3;

	GetIndex(dim1,dim2,dim3,x1,x2,x3);
	if (bins.Valid(dim1,dim2,dim3))
		return bins.Data(dim1,dim2,dim3);
	else
		return 0.0;
}

void RF3D::SetCdf()
{
	double	s=0;
	int	j=0;
	int		l,m,n;

	cdf_d = 1/(((double)bins.size())*10.0);

	cdf_bins.SetRowVector((int)(1/cdf_d)+1);
	cdf_bins.Zero();

	normalize();
	for (l=0; l<bins.Dim1(); l++)
	for (m=0; m<bins.Dim2(); m++)
	for (n=0; n<bins.Dim3(); n++)
	{
		s += bins(l,m,n);
		while (cdf_d*double(j)<s && j<cdf_bins.size())
		{
			cdf_bins(j) = bins.GetIndex(l,m,n);
			j++;
		}
	}
}

bool RF3D::normalize(void)
{
	int		i,j,k;
	double	sum;

	sum = 0.0;
	for (i=0; i<bins.Dim1(); i++)
		for (j=0; j<bins.Dim2(); j++)
			for (k=0; k<bins.Dim3(); k++)
				sum += bins.Data(i,j,k);
	if (sum>0.0)
	{
		for (i=0; i<bins.Dim1(); i++)
			for (j=0; j<bins.Dim2(); j++)
				for (k=0; k<bins.Dim3(); k++)
					bins(i,j,k) = bins(i,j,k)/sum;
		return true;
	}
	else
		return false;
}
